import com.cyberbotics.webots.controller.DifferentialWheels;
import com.cyberbotics.webots.controller.DistanceSensor;

public class BangBangPushBall extends DifferentialWheels {
	private static final int THRESHOLD_FRONT = 2000; // <2000 --> there is a ball in front
	private static final int THRESHOLD_SIDE = 600;

	private static int TIME_STEP = 16;

	private static int S_MID_LEFT = 0; // Sensor mid left
	private static int S_FRONT_LEFT = 1; // Sensor front left
	private static int S_FRONT_RIGHT = 2; // Sensor front right
	private static int S_MID_RIGHT = 3; // Sensor mid right
	private static int MAX_SPEED = 1000; // max. motor speed

	private static int CARE_LOW = 400; // min. motor speed
	private static int CARE_HIGH = 600; // max. motor speed

	private DistanceSensor[] _distanceSensors; // Array with all distance sensors

	/**
	 * Constructor
	 */
	public BangBangPushBall() {
		super();
		initSensors();
	}

	protected void initSensors() {
		// get distance sensors and save them in array
		_distanceSensors = new DistanceSensor[] { getDistanceSensor("ps6"), getDistanceSensor("ps7"), getDistanceSensor("ps0"), getDistanceSensor("ps1") };
		for (int i = 0; i < 4; i++) {
			_distanceSensors[i].enable(10);
		}
	}

	/**
	 * Robot drives to the right
	 */
	private void driveRight() {
		setSpeed(CARE_HIGH, CARE_LOW);
	}


	/**
	 * Robot drives to the left
	 */
	private void driveLeft() {
		setSpeed(CARE_LOW, CARE_HIGH);
	}

	private void turnLeft() {
		setSpeed((-1) * CARE_LOW, CARE_HIGH);
	}

	private void turnRight() {
		setSpeed(CARE_LOW, (-1) * CARE_HIGH);
	}

	/**
	 * Robot drives forward
	 */
	private void driveForward() {
		setSpeed(MAX_SPEED, MAX_SPEED);
	}

	/**
	 * User defined function for initializing and running the BangBangFollowTheWall class
	 */
	private void align() {
		while (step(TIME_STEP) != -1) {
			// check that there is a ball in front of the robot
			if (_distanceSensors[S_FRONT_LEFT].getValue() < THRESHOLD_FRONT && _distanceSensors[S_FRONT_RIGHT].getValue() < THRESHOLD_FRONT) {
				// if a ball is rather on the left turn left
				if (_distanceSensors[S_MID_LEFT].getValue() > THRESHOLD_SIDE) {
					turnLeft();
				}
				// if a ball is rather on the right turn right
				else if (_distanceSensors[S_MID_RIGHT].getValue() > THRESHOLD_SIDE) {
					turnRight();
				}
				// else drive forward
				else {
					driveForward();
				}
			}
			// follow the ball, which is more or less in contact with the robot
			else if (_distanceSensors[S_FRONT_LEFT].getValue() > _distanceSensors[S_FRONT_RIGHT].getValue() && _distanceSensors[S_FRONT_LEFT].getValue() > 1000) {
				driveLeft();
			} else if (_distanceSensors[S_FRONT_LEFT].getValue() < _distanceSensors[S_FRONT_RIGHT].getValue() && _distanceSensors[S_FRONT_RIGHT].getValue() > 1000) {
				driveRight();
			}
		}
	}

	/**
	 * Main method - in this method an instance of the controller is created and the method to launch the robot is called.
	 *
	 * @param args
	 */
	public static void main(String[] args) {
		BangBangPushBall controller = new BangBangPushBall();
		controller.align();
	}

}